/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      MotorProtect.c
 * @author    Fortiortech  Appliction Team
 * @date      2022-07-13
 * @brief     This file contains  MotorProtect function used for Motor Control.
 */

#include <FU68xx_6.h>
#include <Myproject.h>
#define TSDTemp1 130 ///< 内部温度传感器 温度值>130度 限额75%
#define TSDTemp2 110 ///< 内部温度传感器 温度值>130度 限额75%
#define TSDTemp3 100 ///< 内部温度传感器 温度值>130度 限额75%
/* Private variables ---------------------------------------------------------*/
FaultStateType data mcFaultSource; ///< 故障类型
FaultCurrentVarible idata mcCurVarible;
FaultVarible xdata Fault;          ///< 故障检测相关结构体变量
FaultRecoverTypedef xdata Restart; ///< 故障恢复重启控制相关结构体变量

uint16 xdata NTC_TABLE[] = // 温度表b=3380k
    {
        // 32767, 34540, 34438, 34332, 34221, 34106, 33986, 33861, 33731, 33596, // -40~-30度
        // 33456, 33311, 33160, 33004, 32843, 32676, 32503, 32325, 32140, 31950, // -30~-20度
        // 32767, 31553, 31345, 31132, 30913, 30688, 30457, 30220, 29977, 29728, // -20~-10度
        // 29474, 29213, 28946, 28673, 28396, 28113, 27825, 27532, 27235, 26932, // -10~0度
        26626, 26316, 26002, 25684, 25363, 25038, 24710, 24379, 24045, 23709, // 0~10度
        23370, 23029, 22687, 22343, 21998, 21652, 21306, 20959, 20612, 20265, // 10~20度
        19919, 19574, 19230, 18887, 18545, 18204, 17865, 17528, 17192, 16859, // 20~30度
        16529, 16202, 15878, 15557, 15239, 14926, 14615, 14309, 14007, 13708, // 30~40度
        13414, 13123, 12836, 12553, 12275, 12001, 11731, 11466, 11205, 10949, // 40~50度
        10698, 10451, 10208, 9971, 9737, 9509, 9285, 9065, 8850, 8639,        // 50~60度
        8433, 8234, 8039, 7849, 7663, 7481, 7303, 7130, 6960, 6794,           // 60~70度
        6632, 6474, 6319, 6168, 6020, 5876, 5735, 5598, 5464, 5334,           // 70~80度
        5206, 5082, 4961, 4844, 4729, 4616, 4507, 4401, 4297, 4196,           // 80~90度
        4097, 4000, 3905, 3814, 3724, 3636, 3551, 3468, 3387, 3308,           // 90~100度
        3231, 3156, 3084, 3013, 2944, 2877, 2811, 2747, 2685, 2625,           // 100~110度
        2565, 2508, 2451, 2396, 2343, 2291, 2240, 2191, 2142, 2095,           // 110~120度
        2049, 2004, 1961, 1918, 1877, 1836, 1797, 1758, 1721, 1684,           // 120~130度
        1648, 1613, 1579, 1546, 1514, 1482, 1451, 1421, 1392, 1363,           // 130~140度
        1335, 1308, 1281, 1255, 1230, 1205, 1181, 1157, 1134, 1112, 1090      // 140~150度
};

/**
 * @brief     过温检测
 * @date      2022-07-14
 */
uint8 NTC_Temp = 0;
uint8 TSD_Temp = 0;
uint8 TempeDerating = 0; /* 过温降额标志位 */
void Fault_NTCTemperature(void)
{
    uint8 i = 0;

    static uint8 Temp_time130 = 0;
    static uint8 Temp_time110 = 0;
    uint8 Left = 0;
    uint8 Right = 151;
    uint8 Mid = 0;

    if (mcFocCtrl.NTCTempFlt >= NTC_TABLE[0])
    {
        NTC_Temp = 0;
    }
    else if (mcFocCtrl.NTCTempFlt <= NTC_TABLE[150])
    {
        NTC_Temp = 150;
    }
    else
    {
        while (Left <= Right) // 二分查找
        {
            Mid = Left + ((Right - Left) >> 1);
            if ((mcFocCtrl.NTCTempFlt <= NTC_TABLE[Mid]) && (mcFocCtrl.NTCTempFlt >= NTC_TABLE[Mid + 1]))
            {
                i = Mid;
                break;
            }
            else if ((mcFocCtrl.NTCTempFlt >= NTC_TABLE[Mid]) && (mcFocCtrl.NTCTempFlt <= NTC_TABLE[Mid - 1]))
            {
                i = Mid - 1;
                break;
            }
            else if (mcFocCtrl.NTCTempFlt > NTC_TABLE[Mid])
            {
                Right = Mid - 1;
            }
            else
            {
                Left = Mid + 1;
            }
        }
        NTC_Temp = i; /* 获取NTC温度值 */
    }
    if (mcFaultSource == FaultNoSource)
    {

        if (NTC_Temp >= TSDTemp1) /* 温度超过145度 */
        {
            if (Temp_time130 < 200)
            {
                Temp_time130++;
            }
            else
            {
                mcFaultSource = FaultNtcOTErr;
                TempeDerating = 0;
            }
        }
        else if (NTC_Temp >= TSDTemp2) /* 温度超过140度 */
        {
            Temp_time130 = 0;
            if (Temp_time110 < 200)
            {
                Temp_time110++;
            }
            else
            {
                TempeDerating = 1;
            }
        }
        else
        {
            if (Temp_time110)
                Temp_time110--;
        }
    }
}

/**
 * @brief     TSD过温检测
 * @date      2022-07-14
 */
void Fault_TSDTemperature(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if (mcFocCtrl.MCU_TEMP >= TempProtValue) // 过温保护
        {
            if (Fault.Temperature.TSDDetecCnt < 10000)
            {
                Fault.Temperature.TSDDetecCnt++;
            }
            else
            {
                Fault.Temperature.TSDDetecCnt = 0;
                mcFaultSource = FaultTSD;
            }
        }
        else
        {
            Fault.Temperature.TSDDetecCnt = 0;
        }
    }
}

/**
 * @brief     过欠压检测
 * @date      2022-07-14
 */
void Fault_Voltage(void)
{
    if (Fault.Voltage.DectDealyCnt < 50)
    {
        Fault.Voltage.DectDealyCnt++;
    }
    else if (mcFaultSource == FaultNoSource)
    {
        /* 过压检测 */
        if (mcFocCtrl.mcDcbusFlt > Fault.Voltage.OverVoltageVal)
        {
            Fault.Voltage.OverVoltDetecCnt += 1;

            if (Fault.Voltage.OverVoltDetecCnt >= 100)
            {
                Fault.Voltage.OverVoltDetecCnt = 0;
                mcFaultSource = FaultOverVoltageDC;
            }
        }
        else
        {
            if (Fault.Voltage.OverVoltDetecCnt > 0)
            {
                Fault.Voltage.OverVoltDetecCnt--;
            }
        }

        if (mcFocCtrl.mcDcbusFlt < Fault.Voltage.UnderVoltageVal)
        {
            Fault.Voltage.UnderVoltDetecCnt += 1;

            if (Fault.Voltage.UnderVoltDetecCnt >= 100)
            {
                Fault.Voltage.UnderVoltDetecCnt = 0;
                mcFaultSource = FaultUnderVoltageDC;
            }
        }
        else
        {
            if (Fault.Voltage.UnderVoltDetecCnt > 0)
            {
                Fault.Voltage.UnderVoltDetecCnt--;
            }
        }
    }
}

/**
 * @brief     堵转检测
 * @date      2022-07-14
 */
uint16 a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11;
uint16 b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11;
void Fault_Stall(void)
{
    a1 = OVCTR1;
    a2 = OVCTR2;
    a3 = OVCTR3;
    a4 = OVCTR4;
    a5 = OVCTR5;
    a6 = OVCTR6;
    a7 = OVCTR7;
    a8 = OVCTR8;
    a9 = OVCTR9;
    a10 = OVCTR10;
    a11 = OVCTR11;
    b1 = OVC1;
    b2 = OVC2;
    b3 = OVC3;
    b4 = OVC4;
    b5 = OVC5;
    b6 = OVC6;
    b7 = OVC7;
    b8 = OVC8;
    b9 = OVC9;
    b10 = OVC10;
    b11 = OVC11;

    if (((mcFocCtrl.mcDcbusRef < OVCTR0) && (mcFocCtrl.mcADCCurrentbus >= OVC0)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR1) && (mcFocCtrl.mcADCCurrentbus >= OVC1)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR2) && (mcFocCtrl.mcADCCurrentbus >= OVC2)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR3) && (mcFocCtrl.mcADCCurrentbus >= OVC3)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR4) && (mcFocCtrl.mcADCCurrentbus >= OVC4)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR5) && (mcFocCtrl.mcADCCurrentbus >= OVC5)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR6) && (mcFocCtrl.mcADCCurrentbus >= OVC6)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR7) && (mcFocCtrl.mcADCCurrentbus >= OVC7)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR8) && (mcFocCtrl.mcADCCurrentbus >= OVC8)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR9) && (mcFocCtrl.mcADCCurrentbus >= OVC9)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR10) && (mcFocCtrl.mcADCCurrentbus >= OVC10)) ||
        ((mcFocCtrl.mcDcbusRef < OVCTR11) && (mcFocCtrl.mcADCCurrentbus >= OVC11)))

    {
        Fault.Stall.SpeedMinCnt++;
        if (Fault.Stall.SpeedMinCnt >= 100)
        {
            Fault.Stall.SpeedMinCnt = 0;
            //            if (isCtrlPowOn)
            //                mcFaultSource = FaultSoftOVCurrent;
        }
    }
    else
    {
        if (Fault.Stall.SpeedMinCnt > 0)
        {
            Fault.Stall.SpeedMinCnt--;
        }
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_PhaseLoss
    Description    : 缺相检测
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_PhaseLoss(void)
{
    if (mcState == mcRun)
    {
    }
}

/**
    @brief      偏置电压检测
*/
void Fault_GetCurrentOffset(void)
{
    if (mcCurOffset.OffsetFlag == 1)
    {
#if (VHALF_OUT_EN == Enable) // 有加VHALF偏置，理论值为16383
        {
#if (Shunt_Resistor_Mode == Single_Resistor) // 单电阻模式
            {
                if ((mcCurOffset.Iw_busOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IuOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.IvOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IvOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IuOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.IvOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IvOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#endif
        }
#else                                          // 没加VHALF偏置,理论值在0
        {
#if (Shunt_Resistor_Mode == Single_Resistor)   // 单电阻模式
            {
                if (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue)
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue) || (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#endif
        }
#endif
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Detection
    Description    : 故障检测与保护,扫描周期默认为1ms
                     所有故障发送只进行 故障码 赋值
                     禁止在状态机以外地方进行状态跳转
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_Detection(void)
{
#if (OverTPProtectEn == 1)
    {
        Fault_TSDTemperature();
        Fault_NTCTemperature();
    }
#endif
#if (VoltageProtectEn == 1)
    {
        Fault_Voltage();
    }
#endif
#if (StallProtectEn == 1)
    {
        Fault_Stall();
    }
#endif
#if (ProtectRecoveryEn == 1)
    {
        Fault_Recovery();
    }
#endif
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Recovery
    Description    : 故障恢复，条件满足只清除故障码，状态跳转由状态机执行
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_Recovery(void)
{
    if (mcState == mcFault)
    {
#if (OV_RecoveryTimes) /* DC电压保护恢复 */
        if (((mcFocCtrl.mcDcbusFlt > UNDER_VOLTAGE_RECOVER) && (mcFocCtrl.mcDcbusFlt < OVER_VOLTAGE_RECOVER)) && (mcFaultSource == FaultUnderVoltageDC || mcFaultSource == FaultOverVoltageDC))
        {
            if (Restart.OV_Times <= OV_RecoveryTimes)
            {
                if (Restart.DC_DelayTcnt < OV_RecoveryDelayTime)
                {
                    Restart.DC_DelayTcnt++;
                }
                else
                {
                    Restart.OV_Times++;
                    Restart.DC_DelayTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }
        else
        {
            Restart.DC_DelayTcnt = 0;
        }
#endif
#if (OT_RecoveryTimes)
        /* TSD过温保护恢复 */
        if (mcFaultSource == FaultTSD)
        {
            if (mcFocCtrl.MCU_TEMP <= TempRecoverValue)
            {
                if (Restart.OT_Times < OT_RecoveryTimes)
                {
                    if (Restart.OT_DelayTcnt < OT_RecoveryDelayTime)
                    {
                        Restart.OT_DelayTcnt++;
                    }
                    else
                    {
                        Restart.OT_Times++;
                        Restart.OT_DelayTcnt = 0;
                        mcFaultSource = FaultNoSource;
                    }
                }
            }
        }
        /* NTC过温保护恢复 */
        if (mcFaultSource == FaultNtcOTErr)
        {
            if (NTC_Temp <= TSDTemp3) // 温度低于100°
            {
                if (Restart.OT_Times < OT_RecoveryTimes)
                {
                    if (Restart.OT_DelayTcnt < OT_RecoveryDelayTime)
                    {
                        Restart.OT_DelayTcnt++;
                    }
                    else
                    {
                        Restart.OT_Times++;
                        Restart.OT_DelayTcnt = 0;
                        TempeDerating = 0;
                        mcFaultSource = FaultNoSource;
                    }
                }
            }
        }

#endif
#if (OC_RecoveryTimes)
        /* 软件过流恢复 */
        if (mcFaultSource == FaultSoftOVCurrent || mcFaultSource == FaultHardOVCurrent)
        {
            if (Restart.SWOC_Times < OC_RecoveryTimes)
            {
                switch (Restart.SWOC_Times)
                {
                case 0:
                    Restart.SWOC_RecoverytTiem = OC_RecoveryDelayTime;
                    break;
                case 1:
                    Restart.SWOC_RecoverytTiem = OC_RecoveryDelayTime << 1;
                    break;
                case 2:
                    Restart.SWOC_RecoverytTiem = OC_RecoveryDelayTime + (OC_RecoveryDelayTime << 1);
                    break;
                case 3:
                    Restart.SWOC_RecoverytTiem = OC_RecoveryDelayTime << 2;
                    break;
                default:
                    Restart.SWOC_RecoverytTiem = OC_RecoveryDelayTime + (OC_RecoveryDelayTime << 2);
                    break;
                }

                if (Restart.SWOC_DelayTcnt < OC_RecoveryDelayTime)
                {
                    Restart.SWOC_DelayTcnt++;
                }
                else
                {
                    Restart.SWOC_Times++;
                    Restart.SWOC_DelayTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }
#endif
    }
}
